/**********************************************************************************************************************
 *
 * Project:    mGameEngine
 * File:       body.cpp
 * Author:     twoja stara
 * Created on: 2009-09-25
 *
 **********************************************************************************************************************/

#include "body.h"
#include "nxustreaming.h"

namespace mGameEngine
{
namespace Physics
{

Body::Body() :
    Actor(),
    _shapes(), _restitution(0.5f), _staticFriction(0.5f), _dynamicFriction(0.4f), _actor(NULL), _material(NULL)
{
}

Body::Body(Shape *shape, float restitution, float staticFriction, float dynamicFriction) :
    Actor(),
    _shapes(), _restitution(restitution), _staticFriction(staticFriction), _dynamicFriction(dynamicFriction),
    _actor(NULL), _material(NULL)
{
    addCollisionShape(shape);
}

Body::~Body()
{
    // delete shapes
    while(!_shapes.empty())
    {
        delete _shapes.front();
        _shapes.pop_front();
    }

    // delete actor and material
    if(_actor && _material)
    {
        _actor->getScene().releaseActor(*_actor);
        _material->getScene().releaseMaterial(*_material);
    }
}

NxActor *Body::_createJointActor(Joint *joint) const
{
    return _actor;
}

NxShapeDesc *Body::_createShapeDescriptor(Shape *shape) const
{
    // get type - blah downcasting ;P
    ShapeType type = shape->getShapeType();

    // no shape
    if(type == SHAPE_NONE)
    {
        return NULL;
    }

    // plane
    else if(type == SHAPE_PLANE)
    {
        Plane *plane = (Plane *)shape;

        NxPlaneShapeDesc *planeDesc = new NxPlaneShapeDesc();
        planeDesc->userData = (void *)shape;
        planeDesc->materialIndex = _material ? _material->getMaterialIndex() : 0;
        planeDesc->normal = NxVec3(plane->getNormal());
        planeDesc->d = dot(plane->getNormal(), plane->getOrigin());

        return planeDesc;
    }

    // box
    else if(type == SHAPE_BOX)
    {
        Box *box = (Box *)shape;

        Matrix4x4 m = Matrix4x4(box->getOrientation(), box->getCenter());

        NxBoxShapeDesc *boxDesc = new NxBoxShapeDesc();
        boxDesc->userData = (void *)shape;
        boxDesc->materialIndex = _material ? _material->getMaterialIndex() : 0;
        boxDesc->localPose.setColumnMajor44(m);
        boxDesc->dimensions.set(0.5f * box->getSize());
        return boxDesc;
    }

    // sphere
    else if(type == SHAPE_SPHERE)
    {
        Sphere *sphere = (Sphere *)shape;

        Matrix4x4 m = Matrix4x4(Quaternion(), sphere->getCenter());

        NxSphereShapeDesc *sphereDesc = new NxSphereShapeDesc();
        sphereDesc->userData = (void *)shape;
        sphereDesc->materialIndex = _material ? _material->getMaterialIndex() : 0;
        sphereDesc->localPose.setColumnMajor44(m);
        sphereDesc->radius = sphere->getRadius();
        return sphereDesc;
    }

    // capsule
    else if(type == SHAPE_CAPSULE)
    {
        Capsule *capsule = (Capsule *)shape;

        const Vector3 &a = capsule->getStart(), &b = capsule->getEnd();
        Matrix4x4 m = Matrix4x4(Quaternion(b - a) * Quaternion(0.0f, 0.0f, -M_1_SQRT2, M_1_SQRT2), 0.5f * (a + b));
        float h = length(b - a);

        NxCapsuleShapeDesc *capsuleDesc = new NxCapsuleShapeDesc();
        capsuleDesc->userData = (void *)shape;
        capsuleDesc->materialIndex = _material ? _material->getMaterialIndex() : 0;
        capsuleDesc->localPose.setColumnMajor44(m);
        capsuleDesc->height = h;
        capsuleDesc->radius = capsule->getRadius();
        return capsuleDesc;
    }

    // convex mesh
    else if(type == SHAPE_CONVEX_MESH)
    {
        ConvexMesh *convex = (ConvexMesh *)shape;
        const Vector<Vector3> &v = convex->getVertices();

        NxPhysicsSDK *physx = NxCreatePhysicsSDK(NX_PHYSICS_SDK_VERSION);
        NxCookingInterface *cooking = NxGetCookingLib(NX_PHYSICS_SDK_VERSION);
        cooking->NxInitCooking();

        NxConvexMeshDesc meshDesc;
        meshDesc.numVertices = v.size();
        meshDesc.pointStrideBytes = sizeof(Vector3);
        meshDesc.points = &v[0];
        meshDesc.flags = NX_CF_COMPUTE_CONVEX;

        NxuMemoryWriteBuffer buf;
        cooking->NxCookConvexMesh(meshDesc, buf);

        NxConvexShapeDesc *convexDesc = new NxConvexShapeDesc();
        convexDesc->userData = (void *)shape;
        convexDesc->materialIndex = _material ? _material->getMaterialIndex() : 0;
        convexDesc->meshData = physx->createConvexMesh(NxuMemoryReadBuffer(buf.data));
    
        cooking->NxCloseCooking();

        return convexDesc;
    }

    // heightfield shape
    else if(type == SHAPE_HEIGHTFIELD)
    {
        Heightfield *hf = (Heightfield *)shape;
        const Vector3 &size = hf->getSize();
        const Vector3 &position = hf->getPosition();
        uint cols = hf->getColumns();
        uint rows = hf->getRows();
        const float *heights = hf->getHeights();

        NxPhysicsSDK *physx = NxCreatePhysicsSDK(NX_PHYSICS_SDK_VERSION);

        NxHeightFieldDesc hfDesc;
        hfDesc.nbColumns = cols;
        hfDesc.nbRows = rows;
        hfDesc.verticalExtent = -1000.0f;
        uint sz = cols * rows;
        hfDesc.samples = malloc(sizeof(NxU32) * sz);
        hfDesc.sampleStride = sizeof(NxU32);
        for(uint i = 0; i != sz; ++i)
        {
            NxHeightFieldSample *smpl = (NxHeightFieldSample *)hfDesc.samples + i;
            smpl->height = int16(32767.0f * heights[i]);
            smpl->materialIndex0 = smpl->materialIndex1 = 1;
            smpl->tessFlag = 0;
        }
        NxHeightField *nxhf = physx->createHeightField(hfDesc);
        free(hfDesc.samples);

        NxHeightFieldShapeDesc *hfshDesc = new NxHeightFieldShapeDesc();
        hfshDesc->heightField = nxhf;
        hfshDesc->columnScale = size.x / cols;
        hfshDesc->rowScale = size.y / rows;
        hfshDesc->heightScale = size.z / 32767.0f;
        hfshDesc->localPose.setColumnMajor44(Matrix4x4(0.0f, 0.0f, 1.0f, 0.0f,
                                                       1.0f, 0.0f, 0.0f, 0.0f,
                                                       0.0f, 1.0f, 0.0f, 0.0f,
                                                       0.0f, 0.0f, 0.0f, 1.0f));
        return hfshDesc;
    }

    // undefined shape
    else
    {
        return NULL;
    }
}

void Body::addCollisionShape(Shape *shape)
{
    // no null shapes
    if(!shape)
    {
        return;
    }

    _shapes.push_back(shape);
    _shapes.unique();

    // create PhysX shape
    if(_actor)
    {
        NxShapeDesc *shapeDesc = _createShapeDescriptor(shape);
        if(shapeDesc)
        {
            _actor->createShape(*shapeDesc);
        }
    }
}

void Body::removeCollisionShape(Shape *shape)
{
    _shapes.remove(shape);

    // delete PhysX shape
    if(_actor)
    {
        NxShape * const *sh;
        for(sh = _actor->getShapes(); sh && (*sh)->userData != shape; ++sh);
        _actor->releaseShape(**sh);
    }

    // delete actor and material if no shapes
    if(_actor && _material && _shapes.empty())
    {
        _actor->getScene().releaseActor(*_actor);
        _actor = NULL;
        _material->getScene().releaseMaterial(*_material);
        _material = NULL;

        _created = false;
    }
}

void Body::setRestitution(float restitution)
{
    // only physical values
    if(restitution < 0.0f || restitution > 1.0f)
    {
        return;
    }

    _restitution = restitution;

    if(_material)
    {
        _material->setRestitution(restitution);
    }
}

void Body::setStaticFriction(float staticFriction)
{
    // only physical values
    if(staticFriction < 0.0f || staticFriction > 1.0f)
    {
        return;
    }

    _staticFriction = staticFriction;

    if(_material)
    {
        _material->setStaticFriction(staticFriction);
    }
}

void Body::setDynamicFriction(float dynamicFriction)
{
    // only physical values
    if(dynamicFriction < 0.0f || dynamicFriction > 1.0f)
    {
        return;
    }

    _dynamicFriction = dynamicFriction;

    if(_material)
    {
        _material->setDynamicFriction(dynamicFriction);
    }
}

void Body::putForce(const Vector3 & force, const Vector3 & radius)
{
}

void Body::putImpulse(const Vector3 & impulse, const Vector3 & radius)
{
}

void Body::putAcceleration(const Vector3 &acc, const Vector3 &radius)
{
}

void Body::create(NxScene *scene) const
{
    // no shapes - no body
    if(_shapes.empty())
    {
        return;
    }

    // already created
    if(_created)
    {
        return;
    }

    // create material
    NxMaterialDesc materialDesc;
    materialDesc.restitution = _restitution;
    materialDesc.staticFriction = _staticFriction;
    materialDesc.dynamicFriction = _dynamicFriction;
    _material = scene->createMaterial(materialDesc);

    // create actor descriptor
    NxActorDesc desc;

    // create shapes descriptors
    for(List<Shape *>::ConstIterator i = _shapes.begin(); i != _shapes.end(); ++i)
    {
        Shape *shape = *i;
        NxShapeDesc *shapeDesc = _createShapeDescriptor(shape);
        if(shapeDesc)
        {
            desc.shapes.push_back(shapeDesc);
        }
    }

    // create actor
    _actor = scene->createActor(desc);

    // delete descriptors
    for(uint i = 0; i != desc.shapes.size(); ++i)
    {
        delete desc.shapes[i];
    }

    // problems while creating actor
    if(!_actor)
    {
        return;
    }

    // actor is created
    _created = true;
}

void Body::setConnectedObject(void *object)
{
    _object = object;

    if(_actor)
    {
        _actor->userData = object;
    }
}

Matrix4x4 Body::getTransformation() const
{
    Matrix4x4 t(1.0f);

    if(_actor)
    {
        NxMat34 m = _actor->getGlobalPose();
        m.getColumnMajor44(t._m);
    }

    return t;
}

void Body::setTransformation(const Matrix4x4 &t)
{
    if(_actor)
    {
        NxMat34 m;
        m.setColumnMajor44(t);
        _actor->setGlobalPose(m);
    }
}

Vector3 Body::getPointVelocity(const Vector3 &p)
{
    Vector3 v;
    
    if(_actor)
    {
        const NxVec3 &nv = _actor->getLocalPointVelocity(NxVec3(p));
        v.x = nv.x;
        v.y = nv.y;
        v.z = nv.z;
    }

    return v;
}

}
}
